#!/usr/bin/env python
# -*- coding: utf-8 -*-

import requests
import json
from socket import *
import time
import math

"""
整个程序包含以下几个过程。
获取局域网机器人广播，然后获取机器人Token。
获取机器人当前基本信息，获取机器人当前导航坐标，然后发布导航任务，机器人移动到前方0.5m处，并等待任务完成。
使用速度控制API，让机器人原地旋转1秒。
重置机器人遥控速度参数
这几个API分别包含了几种API的请求方式，GET, POST, PUT, DELETE。其他API调用和这几个例子一样。
"""


if __name__ == '__main__':
    # 获取机器人UDP广播
    udp_server = socket(AF_INET,SOCK_DGRAM)
    udp_server.bind(("0.0.0.0", 22002))
    udp_server.settimeout(60)
    data, addr = udp_server.recvfrom(1024)
    data = json.loads(data.decode("utf-8"))
    print(json.dumps(data, indent=4))
    # 获取机器人token，采用默认的用户名密码
    print("Start get robot token")
    robot_ip = addr[0]
    api_url = "http://{ip}:3546/api/v1".format(ip=robot_ip)
    res = requests.get("{api_url}/token?username=admin&password=admin".format(api_url=api_url))
    token = res.json()
    # 获取机器人基本信息
    print("Start get robot info")
    res = requests.get("{api_url}/system/info?token={token}".format(
        api_url=api_url,
        token=token["token"]
    ))
    # 打印当前电压
    print("Current battery: " + str(res.json()["battery"]))
    time.sleep(3)
    # 执行前先开启机器人导航
    # 获取当前坐标
    print("Start get robot pose")
    res = requests.get("{api_url}/navigation/pose?token={token}".format(
        api_url=api_url,
        token=token["token"]
    ))
    pose = res.json()
    # 打印当前坐标
    print("Current pose: x {x} y {y} angle {angle}".format(
        x=pose["x"],
        y=pose["y"],
        angle=pose["angle"]
    ))
    time.sleep(3)
    print("Start move to target point")
    # 移动到目标,向前移动0.5m
    res = requests.post("{api_url}/navigation/start_nav_task?token={token}".format(
            api_url=api_url,
            token=token["token"]
        ), 
        json={
            "x": pose["x"] + 0.5 * math.cos(pose["angle"]),
            "y": pose["y"] + 0.5 * math.sin(pose["angle"]),
            "theta": pose["angle"]
        }
    )
    task_info = res.json()
    # 通过轮询等待任务执行完成
    while True:
        res1 = requests.get("{api_url}/task?token={token}&id={task_id}".format(
            api_url=api_url,
            token=token["token"],
            task_id=task_info["id"]
        ))
        info = res1.json()
        if info["state"] == "COMPLETE" or info["state"] == "CANCELLED" or info["state"] == "ERROR":
            break
        print("Waitting for task complete...")
        time.sleep(1)

    # 让机器人原地旋转1s
    res = requests.put("{api_url}/system/speed?token={token}".format(
            api_url=api_url,
            token=token["token"]
        ),
        json={
            "speed_x": 0, # 机器人向前的速度
            "speed_y": 0, # 机器人向左平移速度，仅全向轮可用
            "speed_angle": 1.0, # 机器人旋转角速度
        }
    )
    time.sleep(1)
    # 停止旋转
    res = requests.put("{api_url}/system/speed?token={token}".format(
            api_url=api_url,
            token=token["token"]
        ),
        json={
            "speed_x": 0, # 机器人向前的速度
            "speed_y": 0, # 机器人向左平移速度，仅全向轮可用
            "speed_angle": 0, # 机器人旋转角速度
        }
    )
    # 重置机器人最大遥控速度 max_control_speed
    res = requests.delete("{api_url}/system/config?token={token}&key=max_control_speed".format(
        api_url=api_url,
        token=token["token"]
    ))






    